#include "graph_optimizer/edge/edge_gnss.hpp"

namespace g2o
{
    EdgeGnss::EdgeGnss() : BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexP>()
    {
        information().setIdentity();
        setMeasurement(Eigen::Vector3d::Zero());
    }
    bool EdgeGnss::read(std::istream &is)
    {
        // read measurement
        Eigen::Vector3d meas;
        for (int i = 0; i < 3; i++)
            is >> meas[i];
        setMeasurement(meas);
        // read covariance matrix (upper triangle)
        if (is.good())
        {
            for (int i = 0; i < information().rows(); i++)
                for (int j = i; j < information().cols(); j++)
                {
                    is >> information()(i, j);
                    if (i != j)
                        information()(j, i) = information()(i, j);
                }
        }
        return !is.fail();
    }
    bool EdgeGnss::write(std::ostream &os) const
    {
        for (int i = 0; i < 3; i++)
            os << measurement()[i] << " ";
        for (int i = 0; i < information().rows(); i++)
            for (int j = i; j < information().cols(); j++)
            {
                os << information()(i, j) << " ";
            }
        return os.good();
    }

    void EdgeGnss::computeError()
    {
        const VertexP *v = static_cast<const VertexP *>(_vertices[0]);
        _error = v->estimate() - _measurement;
    }

    void EdgeGnss::linearizeOplus()
    {
        _jacobianOplusXi = Eigen::Matrix3d::Identity();
    }

    bool EdgeGnss::setMeasurementFromState()
    {
        const VertexP *v = static_cast<const VertexP *>(_vertices[0]);
        _measurement = v->estimate();
        return true;
    }
} // namespace g2o